#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#include <WiFi.h>
#include <esp_now.h>
#include <ESP32CAN.h>
#include <CAN_config.h>
#define LED_pin 2
CAN_device_t CAN_cfg;			  // CAN Config
unsigned long previousMillis = 0; // will store last time a CAN Message was send
const int interval = 20;		  // interval at which send CAN Messages (milliseconds)
const int rx_queue_size = 10;	  // Receive Queue size
//通过CAN通信发送数据
// ID:要发送的消息的ID号
void CAN_Send_Angle_Msg(int ID, float angle1, float angle2)
{
	CAN_frame_t tx_frame;
	tx_frame.FIR.B.FF = CAN_frame_std;
	tx_frame.MsgID = ID;	// ID号
	tx_frame.FIR.B.DLC = 8; //发送数据的长度
	memcpy(&tx_frame.data.u8[0], &angle1, sizeof(angle1));
	memcpy(&tx_frame.data.u8[4], &angle2, sizeof(angle2));
	ESP32Can.CANWriteFrame(&tx_frame);
}
// 设置数据结构体
typedef struct struct_message
{
    String board_name = "";
    float A_motor1_angle = 0;
    float A_motor2_angle = 0;
    float B_motor1_angle = 0;
    float B_motor2_angle = 0;
    float C_motor1_angle = 0;
    float C_motor2_angle = 0;
    float D_motor1_angle = 0;
    float D_motor2_angle = 0;
} struct_message;

struct_message myData;

// 数据接收回调函数
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len)
{
    memcpy(&myData, incomingData, sizeof(myData));
    // myData.motor1_angle = !myData.motor1_angle;
    // if (myData.motor1_angle == 0)
    // {
    //     digitalWrite(LED_pin, LOW);
    // }
    // else
    // {
    //     digitalWrite(LED_pin, HIGH);
    // }
}
//将通过ESP-NOW得到的数据转化为字符串，通过2号硬件串口进行中继发送
void serialTransmitUserCommand()
{
	int ms = 7;
	CAN_Send_Angle_Msg(1, myData.A_motor1_angle, myData.A_motor2_angle);
	delay(ms);
	CAN_Send_Angle_Msg(2, myData.B_motor1_angle, myData.B_motor2_angle);
	delay(ms);
	CAN_Send_Angle_Msg(3, myData.C_motor1_angle, myData.C_motor2_angle);
	delay(ms);
	CAN_Send_Angle_Msg(4, myData.D_motor1_angle, myData.D_motor2_angle);
    delay(ms);
	// String command = "a" + (String)(myData.A_motor1_angle) + "A" + (String)(myData.A_motor2_angle) +
    //                  "b" + (String)(myData.B_motor1_angle) + "B" + (String)(myData.B_motor2_angle) +
    //                  "c" + (String)(myData.C_motor1_angle) + "C" + (String)(myData.C_motor2_angle) +
    //                  "d" + (String)(myData.D_motor1_angle) + "D" + (String)(myData.D_motor2_angle) + "\n";
    // Serial2.print(command);
}
void task_LED(void *pvParam) // 宣告任务LED闪烁
{
	/* 使用ledc通道1，设置频率为1kHz，分辨率为10位 */
	int freq = 1000;					  // 频率
	int channel = 0;					  // 通道
	int resolution = 8;					  // 分辨率
	ledcSetup(channel, freq, resolution); // 设置通道
	ledcAttachPin(LED_pin, channel);	  // 将通道与对应的引脚连接
	while (1)
	{
		// 逐渐变亮
		for (int dutyCycle = 0; dutyCycle <= 255; dutyCycle = dutyCycle + 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}

		// 逐渐变暗
		for (int dutyCycle = 255; dutyCycle >= 0; dutyCycle = dutyCycle - 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}
	}
}
void setup()
{
    xTaskCreate(task_LED, "task_LED", 1000, NULL, 1, NULL);
    Serial.begin(115200);
	Serial.println("Basic Demo - ESP32-Arduino-CAN");
    pinMode(LED_pin, OUTPUT);
    // 初始化 ESP-NOW
    WiFi.mode(WIFI_STA);
    if (esp_now_init() != 0)
    {
        Serial.println("Error initializing ESP-NOW");
        return;
    }
	Serial.println(WiFi.macAddress());
    // 设置接收数据回调函数
    esp_now_register_recv_cb(OnDataRecv);
	CAN_cfg.speed = CAN_SPEED_1000KBPS;
	CAN_cfg.tx_pin_id = GPIO_NUM_17;
	CAN_cfg.rx_pin_id = GPIO_NUM_16;
	CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t));
	// Init CAN Module
	ESP32Can.CANInit();
}
void loop()
{
    serialTransmitUserCommand();
    // Serial.printf("%s,A %lf,A %lf,B %lf,B %lf,C %lf,C %lf,D %lf,D %lf\n", myData.board_name.c_str(), myData.A_motor1_angle, myData.A_motor2_angle, myData.B_motor1_angle, myData.B_motor2_angle, myData.C_motor1_angle, myData.C_motor2_angle, myData.D_motor1_angle, myData.D_motor2_angle);
}
// 同样，我们在开头引入了 ESP-NOW 相关的头文件：WiFi.h 与 esp_now.h。然后定义了传感器的引脚和一个名为 myData 的结构体，该结构体由 3 个不同数据组成，分别是开发板名称（board_name），光线值（motor1_angle）与声音值（motor1_angle），与数据发送端定义的一模一样。
// 我们注册了一个 OnDataRecv() 的数据接收回调函数，该函数反馈了 ESP-NOW 数据的接收状态，当接收到来自其他开发板的消息时，将消息保存到结构体 myData 中，并在串口中打印出接收到的消息。
// 这里 ESP-NOW 的消息的接收属于无阻塞的接收方式，不受延时函数 delay() 的影响，这意味着 loop() 里面可以执行其他任务而不会影响到板间的通讯，同一块板子既可以当做接收方亦可以当做发送方互不影响。这里可以同时接收来自多块板子的信息，但前提是保持相同的结构体，当然你也可以不用结构体用普通的单一类型的数据类型比如字符串之类的，这里用结构体的原因是需要发送的数据类型有多个，使用结构体相对灵活。
